#include "StdAfx.h"
#include "Frame.hxx"
#include "FrameUndistortStep.hxx"

#include "ProcessStream.hxx"

#include "FrameArray.hxx"

#include "AppSetting.hxx"

#include "TrackerModel.hxx"
#include "PipeLine.hxx"
#include "ChannelModel.hxx"

FrameUndistortStep::FrameUndistortStep(ProcessStream* stream):ProcessStep( stream )
{
	_cameraMatrix = 0;
	_distCoeffMatrix = 0;
	
	_mapX = 0;
	_mapY = 0;

	_imageHeader = 0;
}


FrameUndistortStep::~FrameUndistortStep(void)
{
}

void FrameUndistortStep::initialize()
{
	loadParameters();

	_imageHeader = cvCreateImageHeader( cvSize( RAW_FRAME_WIDTH, RAW_FRAME_HEIGHT ), IPL_DEPTH_8U, 3 );

}

void FrameUndistortStep::execute()
{
	FrameArray* frameArray = _stream->getFrameArray();

	Frame* frame = 0;

	bool noAction = true;
	frameArray->lock();

	for( int i = 0; i < frameArray->getPageCount(); i ++ )
	{
		if( frameArray->getFlag( i ) )
		{
			frame = (Frame*)frameArray->getFrameByIndex( i );

			if( frame && frame->ProcessStepFlag == STEP_FRAME_UNDISTORT )
			{
				//frame->ProcessStepFlag = STEP_SYNCRONIZE;

				noAction = false;

				break;
			}
		}
	}
	frameArray->unlock();

	if( noAction ) 
		Sleep( 30 );
	else
	{
		_imageHeader->imageData = frame->FrameImage;

		if( _mapX && _mapY )
            cvRemap( _imageHeader, _imageHeader, _mapX, _mapY );
		
		//ChannelModel* model = getProcessStream()->getPipeLine()->getTrackerModel()->getSubModel( getProcessStream()->getStreamId() );
		//if( model ) model->updateImageData( frame->FrameImage );

		frame->ProcessStepFlag = STEP_FOREGROUND_DETECTION;
		//frame->ProcessStepFlag = STEP_SYNCRONIZE;
	}
}

void FrameUndistortStep::loadParameters()
{

	CvFileStorage* fs = cvOpenFileStorage( __gHostInfo[ getProcessStream()->getStreamId() ].File ,
      0,
      CV_STORAGE_READ
    );

    const char* strtime = cvReadStringByName( fs, 0, "calibration_time",   0 /* default value */   );

    int val = cvReadIntByName( fs, 0, "image_width", 0);
    val = cvReadIntByName( fs, 0, "image_height", 0);
    val = cvReadIntByName( fs, 0, "board_width", 0);
    val = cvReadIntByName( fs, 0, "board_height", 0);
    double dval = cvReadRealByName( fs, 0, "square_size", 0);
    val = cvReadIntByName( fs, 0, "flags", 0);

    _cameraMatrix = (CvMat*)cvReadByName( fs, 0, "camera_matrix", 0);
    _distCoeffMatrix = (CvMat*)cvReadByName( fs, 0, "distortion_coefficients", 0);

    cvReleaseFileStorage( &fs );

    _mapX = cvCreateImage( cvSize( RAW_FRAME_WIDTH, RAW_FRAME_HEIGHT), IPL_DEPTH_32F, 1);
    _mapY = cvCreateImage( cvSize( RAW_FRAME_WIDTH, RAW_FRAME_HEIGHT), IPL_DEPTH_32F, 1);

    cvInitUndistortMap( _cameraMatrix, _distCoeffMatrix, _mapX, _mapY );
}